#include "test/checksum.h"
#include "test/serialProtocol.h"

#ifdef __cplusplus
extern "C" {
#endif

void msg_gps_pack(const gps_info*  _gps_info, uint8_t* msgbuffer){
    uint8_t*  pBuffer = msgbuffer;
    const uint8_t*  pData   = (uint8_t*)_gps_info;
    int i;
    *pBuffer = MSG_HEADER;
    pBuffer ++;
       for (i=0;i<sizeof(gps_info);i++){
        *pBuffer = *pData;
        pBuffer ++ ;
        pData ++;
    }
    //crc 
    uint16_t crc = crc_calculate(msgbuffer+1,sizeof(gps_info));
    * ((uint16_t*)pBuffer)      =  crc;

}


unpackInfo msg_gps_unpack(gps_info*  _gps_info,  uint8_t* msgbuffer){
    //crc 
    //unpackInfo unpackStatus;
    uint16_t crc;
    uint16_t crcExpected;
    uint8_t* pData = (uint8_t*) _gps_info;
    uint8_t* pBuffer= msgbuffer;
    pBuffer ++;
    int i;
    crc=* (uint16_t*)(msgbuffer + sizeof(gps_info)+1);
    crcExpected = crc_calculate(msgbuffer+1,sizeof(gps_info));
    if (crcExpected != crc){
        //unpackStatus = UNPACK_FAIL;
        return UNPACK_FAIL;
    }

    for (i=0;i<sizeof(gps_info);i++){
        *pData = *pBuffer;
        pBuffer ++ ;
        pData ++;
    }
    //unpackStatus = UNPACK_SUCCESS;
    return UNPACK_SUCCESS;
}

void msg_gps_range_pack(const gps_range_info* _gps_range_Info, uint8_t* msgbuffer){
    uint8_t*  pBuffer = msgbuffer;
    const uint8_t*  pData   = (uint8_t*)_gps_range_Info;
    int i;
    *pBuffer = MSG_HEADER;
    pBuffer ++;
    for (i=0;i<sizeof(gps_range_info);i++){
        *pBuffer = *pData;
        pBuffer ++ ;
        pData ++;
    }
    //crc 
    uint16_t crc = crc_calculate(msgbuffer+1,sizeof(gps_range_info));
	*((uint16_t*)pBuffer) = crc;
}

unpackInfo msg_gps_range_unpack(gps_range_info* _gps_range_info, uint8_t* msgbuffer){
    //crc 
    //unpackInfo unpackStatus;
    uint16_t crc;
    uint16_t crcExpected;
    uint8_t* pData = (uint8_t*) _gps_range_info;
    uint8_t* pBuffer= msgbuffer ;
    pBuffer ++;
    int i ;
    crc= *(uint16_t*)(msgbuffer + sizeof(gps_range_info)+1);
    crcExpected = crc_calculate(msgbuffer+1,sizeof(gps_range_info));
    if (crcExpected != crc){
        //unpackStatus = UNPACK_FAIL;
        return UNPACK_FAIL;
    }

    for (i=0;i<sizeof(gps_range_info);i++){
        *pData = *pBuffer;
        pBuffer ++ ;
        pData ++;
    }
    //unpackStatus = UNPACK_SUCCESS;
    return UNPACK_SUCCESS;
}


#ifdef __cplusplus
}
#endif